/*
 * Sight.cpp
 *
 *  Created on: Feb 15, 2010
 *      Author: vuk
 */

#include "Smell.h"
#include <string>
#include <math.h>
#define PI 3.14159265

Smell::Smell():
	ESC(0),
	color(NULL),
	c1("c1"), c2("c2"), c3("c3"),
	cap(1),
//	rawframe(Size(160,120),CV_8SC3),
	filteredFrame(Size(320,240),CV_8SC3)
{
//	cap>>rawframe;
//	resize(rawframe, frame, Size(160,120), 2, 2, INTER_AREA);
//	resize(frame, rawframe, Size(160,120), 2, 2, INTER_AREA);
	ax=bx=0;
	ay=by=-1;
	namedWindow("RawFrameBeacon", CV_WINDOW_AUTOSIZE);
	namedWindow("FrameBeacon", CV_WINDOW_AUTOSIZE);
}

Smell::~Smell()
{
	cvDestroyWindow("RawFrameBeacon");
	cvDestroyWindow("FrameBeacon");
}

void Smell::refresh()
{
	cap>>rawframe;
}

void Smell::showFilteredFrame()
{
	Mat pom;
	cvtColor(filteredFrame, pom, CV_HSV2BGR);
	imshow("FrameBeacon",pom);
}

void Smell::showMiniRawFrame()
{
	Mat pom(Size(160,120),CV_8SC3);
	resize(rawframe, pom, Size(160,120), 1, 1, INTER_NEAREST);
	//cvtColor(pom, pom, CV_HSV2BGR);
	imshow("RawFrameBeacon",pom);
}

void Smell::showRawFrame()
{
	imshow("RawFrameBeacon",rawframe);
}


void Smell::makeFilteredFrame()
{
	//rectangle(frame, Point(0,0), Point(frame.cols, frame.rows), Scalar (0,128,0), -1);
//	imshow("RawFrame",helpframe);
	resize(rawframe, filteredFrame, Size(320,240), 2, 2, INTER_NEAREST);
	cvtColor(filteredFrame, helpFrame, CV_BGR2HSV);
	cvtColor(filteredFrame, filteredFrame, CV_BGR2HSV);

	for(int i=0; i<filteredFrame.rows; i++)
	{
		for(unsigned int j=0; j<filteredFrame.step ; j+=filteredFrame.channels())
		{

			if(c1==helpFrame.data+(i*helpFrame.step)+j)
				c1.fillwidthrepresent(filteredFrame.data+(i*filteredFrame.step)+j);
			else if(c2==helpFrame.data+(i*helpFrame.step)+j)
				c2.fillwidthrepresent(filteredFrame.data+(i*filteredFrame.step)+j);
			else if(c3==helpFrame.data+(i*helpFrame.step)+j)
				c3.fillwidthrepresent(filteredFrame.data+(i*filteredFrame.step)+j);
			else
				*(filteredFrame.data+(i*filteredFrame.step)+j+2)=0;
		}
	}
//	filteredFrame=helpframe;
}

void Smell::changeColors(const char &taster)
{
	switch(taster)
	{
		case('-'): cvDestroyWindow ((color->getName()+" boja").c_str());color=NULL ; break;
		case('8'): if(color)cvDestroyWindow ((color->getName()+" boja").c_str());
				 color=&c1; namedWindow(c1.getName()+" boja", CV_WINDOW_AUTOSIZE); break;
		case('9'): if(color)cvDestroyWindow ((color->getName()+" boja").c_str());
				color=&c2; namedWindow(c2.getName()+" boja", CV_WINDOW_AUTOSIZE); break;
		case('0'): if(color)cvDestroyWindow ((color->getName()+" boja").c_str());
				color=&c3; namedWindow(c3.getName()+" boja", CV_WINDOW_AUTOSIZE); break;
	}
	if(color)
	{
		color->config(taster);
		Mat resizeframe;
		resize(rawframe, resizeframe, Size(160,120), 2, 2, INTER_NEAREST);
		cvtColor(resizeframe, resizeframe, CV_BGR2HSV);
		for(int i=0; i<resizeframe.rows; i++)
		{
			for(unsigned int j=0; j<resizeframe.step ; j+=resizeframe.channels())
			{
				if(*color==resizeframe.data+(i*resizeframe.step)+j)
					color->fillwidthrepresent(resizeframe.data+(i*resizeframe.step)+j);
				else
					*(resizeframe.data+(i*resizeframe.step)+j+2)=0;
			}
		}
		cvtColor(resizeframe, resizeframe, CV_HSV2BGR);
		imshow((color->getName()+" boja").c_str(),resizeframe);
	}
}

bool Smell::isBeaconColor(Mat &workframe, int i, int j, NamedColorRangeHSV &c)
{
    return c == workframe.data + (i * workframe.step) + j
    	&& c == workframe.data + (i * workframe.step) + j + workframe.channels()
    	&& c == workframe.data + (i * workframe.step) + j + 2 * workframe.channels()
    	&& c == workframe.data + (i * workframe.step) + j + 3 * workframe.channels()
    	&& c == workframe.data + ((i + 1) * workframe.step) + j
    	&& c == workframe.data + ((i + 1) * workframe.step) + j + workframe.channels()
    	&& c == workframe.data + ((i + 1) * workframe.step) + j + 2 * workframe.channels()
    	&& c == workframe.data + ((i + 1) * workframe.step) + j + 3 * workframe.channels();
}

void Smell::getGoal()
{
	Mat workframe;
	resize(rawframe, workframe, Size(320,240), 2, 2, INTER_NEAREST);
	cvtColor(workframe, workframe, CV_BGR2HSV);
	ay=by=-1;
	for(int i=0; i<workframe.rows-2; i++)
	{
		for(unsigned int j=0; j<workframe.step-5 ; j+=workframe.channels())
		{
			if(isBeaconColor(workframe, i, j, c1))
				for(int ii=i; ii<workframe.rows-2; ii++)
				{
					if(isBeaconColor(workframe, ii, j, c2))
						for(int iii=ii; iii<workframe.rows-2; iii++)
							if(isBeaconColor(workframe, iii, j, c3))
							{
								ax=j/workframe.channels();
								ay=i;
								break;
							}
					if(ay>=0) break;
				}
			if(ay>=0) break;
		}
		if(ay>=0) break;
	}
	if(ay>=0)
	{
		for(int i=workframe.rows; i>0+2; i--)
		{
			for(unsigned int j=workframe.step; j>0+5 ; j-=workframe.channels())
			{
				if(isBeaconColor(workframe, i, j, c3))
					for(int ii=i;  ii>0+2; ii--)
					{
						if(isBeaconColor(workframe, ii, j, c2))
							for(int iii=ii;  iii>0+2; iii--)
								if(isBeaconColor(workframe, iii, j, c1))
								{
									bx=j/workframe.channels();
									by=i;
									break;
								}
						if(by>=0) break;
					}
				if(by>=0) break;
			}
			if(by>=0) break;
		}
	}
	if(ay>=0 && by>=0)
	{
		cout<<endl<<"AY="<<ay<<endl;
		cout<<"AX="<<ax<<endl;
		cout<<"BY="<<by<<endl;
		cout<<"BX="<<bx<<endl;
		//cout<<"Razlika: "<<by-ay<<endl;
		cout<<"Razlika: "<<by-ay<<"  Distanca "<<5/tan((by-ay)*PI*.125/180)<<endl;//treba 125
	}
}

